/* -*- c++ -*- */
/*
 * Copyright 2004,2006 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gr_framer_sink_xstar.h>
#include <gr_io_signature.h>
#include <cstdio>
#include <stdexcept>

#define VERBOSE 1

inline void
gr_framer_sink_xstar::enter_search()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_search\n");

  d_state = STATE_SYNC_SEARCH;
}

inline void
gr_framer_sink_xstar::enter_radio_header()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_radio_header\n");

  d_state = STATE_IN_RADIO_HEADER;
  d_subframe_byte = 0;
  d_subframe_byte_index = 0;
  d_subframelen = HEADER_BITLEN / 8;
  d_subframelen_cnt = 0;
  d_numsubframe = 0;
}

inline void
gr_framer_sink_xstar::enter_voice_frame()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_voice_frame\n");

  d_state = STATE_IN_VOICE_FRAME;
  d_subframe_byte = 0;
  d_subframe_byte_index = 0;
  d_subframelen = VOICE_BITLEN / 8;
  d_subframelen_cnt = 0;
  ++d_numsubframe;
}

inline void
gr_framer_sink_xstar::enter_data_frame()
{
  if (VERBOSE)
    fprintf(stderr, "@ enter_data_frame\n");

  d_state = STATE_IN_DATA_FRAME;
  d_subframe_byte = 0;
  d_subframe_byte_index = 0;
  d_subframelen = DATA_BITLEN / 8;
  d_subframelen_cnt = 0;
  ++d_numsubframe;
}

gr_framer_sink_xstar_sptr
gr_make_framer_sink_xstar(gr_msg_queue_sptr target_queue)
{
  return gr_framer_sink_xstar_sptr(new gr_framer_sink_xstar(target_queue));
}


gr_framer_sink_xstar::gr_framer_sink_xstar(gr_msg_queue_sptr target_queue)
  : gr_sync_block ("framer_sink_xstar",
		   gr_make_io_signature (1, 1, sizeof(unsigned char)),
		   gr_make_io_signature (0, 0, 0)),
    d_target_queue(target_queue)
{
  enter_search();
}

gr_framer_sink_xstar::~gr_framer_sink_xstar ()
{
}

int
gr_framer_sink_xstar::work (int noutput_items,
			gr_vector_const_void_star &input_items,
			gr_vector_void_star &output_items)
{
  const unsigned char *in = (const unsigned char *) input_items[0];
  int count=0;

  /*
  if (VERBOSE)
    fprintf(stderr,">>> Entering state machine\n");
    */

  while (count < noutput_items){
    switch(d_state) {

    case STATE_SYNC_SEARCH:    // Look for flag indicating beginning of pkt
            /*
      if (VERBOSE)
	fprintf(stderr,"SYNC Search, noutput=%d\n", noutput_items);
        */

      while (count < noutput_items) {
	if (in[count] & 0x2){  // Found it, set up for header decode
	  enter_radio_header();
	  break;
	}
	count++;
      }
      break;

    case STATE_IN_RADIO_HEADER:
    case STATE_IN_VOICE_FRAME:
    case STATE_IN_DATA_FRAME:

      /*
      if (VERBOSE)
	fprintf(stderr,"Packet Build\n");
        */

      while (count < noutput_items) { // shift bits into bytes of subframe one at a time
	d_subframe_byte = (d_subframe_byte << 1) | (in[count++] & 0x1);
	if (d_subframe_byte_index++ == 7) {	  	// byte is full so move to next byte
	  d_subframe[d_subframelen_cnt++] = d_subframe_byte;
	  d_subframe_byte_index = 0;

	  if (d_subframelen_cnt == d_subframelen){ // subframe is complete
            subframe_type_t message_type;
            switch(d_state) {
            case STATE_IN_RADIO_HEADER: message_type = SUBFRAME_HEADER; break;
            case STATE_IN_VOICE_FRAME: message_type = SUBFRAME_VOICE; break;
            case STATE_IN_DATA_FRAME: message_type = SUBFRAME_DATA; break;
            }

            write(2, "<", 1);
            char c = '0' + message_type;
            write(2, &c, 1);
            write(2, ", ", 2);
            write(2, d_subframe, d_subframelen_cnt);
            write(2, ">\n", 2);

            // build a message
            gr_message_sptr msg = gr_make_message(0, 0, 0, d_subframelen_cnt);
            memcpy(msg->msg(), d_subframe, d_subframelen_cnt);


            switch(d_state) {
            case STATE_IN_RADIO_HEADER:
                enter_voice_frame();
                break;

            case STATE_IN_VOICE_FRAME:
                enter_data_frame();
                break;

            case STATE_IN_DATA_FRAME:
                long long tmp = 0;
                int i;
                for (i=0; i<6; ++i) /* Convert to native endian */
                    tmp = (tmp<<8) | (d_subframe[14+i]);

                tmp <<= 16;

                fprintf(stderr, "Found postamble %llx, looking for %llx, mask %llx\n", tmp, d_postamble, d_postamble_mask);

                tmp &= d_postamble_mask;

                if (!memcmp(&tmp, &d_postamble, 8)) {
                    message_type = SUBFRAME_DATAEND;
                    enter_search();
                }
                else
                    enter_voice_frame(); 
                break;
            }

            msg->set_type(message_type);
            d_target_queue->insert_tail(msg);		// send it
            msg.reset();  				// free it up

	    break;
	  }
	}
      }
      break;

    default:
      assert(0);

    } // switch

  }   // while

  return noutput_items;
}

bool
gr_framer_sink_xstar::set_postamble(
  const std::string &postamble)
{
  unsigned len = postamble.length();  // # of bytes in string
  if (len > 64)
    return false;

  d_postamble = 0;

  if (len == 64)
    d_postamble_mask = ~0ULL;
  else
    d_postamble_mask = ~((1 << len) - 1);

  for (unsigned i=0; i < 64; i++){
    d_postamble <<= 1;
    if (i < len)
      d_postamble |= postamble[i] & 1;  // look at LSB only
  }

  return true;
}
